;
; ****************************************************************************************
; * 								QuadRotor Firmware V1
; *
; * (C)2009 by Kharsansky Alan & Vilaseca David
; ****************************************************************************************
;

; Included header file for target AVR typde
.NOLIST
.INCLUDE "m168def.inc"	;Header for ATMEGA168
.INCLUDE "macros.inc" 	;Macros de AVR001
.LIST

; ========================================================================================
;   P O R T S   A N D   P I N S 
; ========================================================================================
.EQU BUZZER = PORTB0
;
; ========================================================================================
;   C O N S T A N T S   T O   C H A N G E 
; ========================================================================================

.EQU FOSC=12000000
.EQU BAUDRATE=57600

;Command Headers

.EQU ANGLES	=	0x11
.EQU SET_POINTS=	0x12
.EQU JOYSTICK=		0x13
.EQU SETPWM = 		0x24
.EQU KPARAMS = 		0x35

;Transmition Commands

.EQU START_DEBUG = 	0x01
.EQU STOP_DEBUG = 	0x04

.EQU MIN = 		0		;Motor  0%
.EQU MAX = 		100;180		;Motor 100% 
.EQU PWMQ = 	50		;Must be measured in flight

;System Flags

.EQU F_START =		0x01		;Start Flag
.EQU F_PID_GO =		0x02		;PID_GO Flag

.EQU F_START_BIT = 	0		;Start Flag BIT
.EQU F_PID_GO_BIT =	1		;PID_GO Flag Bit


.EQU MAX_I = 127			;Integral term MAX clipping value
.EQU MIN_I = -127			;Integral term MI clipping value

; ========================================================================================
;   R E G I S T E R   D E F I N I T I O N S
; ========================================================================================


;R0 and R1 are Reserverd for Hardware Multiplier
;R26-R31 are reserverd for X,Y,Z 
;DON'T USE THEM!

.DEF count = 		R28		;UART parameters index

.DEF pwma = 		R7
.DEF pwmb = 		R8

.DEF pterm = 		R10
.DEF iterm = 		R11
.DEF dterm =		R12

.DEF delta_pwm = 	R13
.DEF roll = 		R14
.DEF istate = 		R9
.DEF sp = 		R15


.DEF rmp = 		R16 		;Multipurpose register
.DEF rmp2 = 		R17 		;Multipurpose register
.DEF txbuff = 		R18 		;Transmition Buffer
.DEF rxbuff = 		R19 		;Reception Buffer

.DEF flags = 		R20		;CAUTION: Do not use it for ANYTHING else

.DEF kp = 		R21		;Must be between R16 - R23 for the HW Multiplier
.DEF kd= 		R22		;Must be between R16 - R23 for the HW Multiplier
.DEF ki= 		R23		;Must be between R16 - R23 for the HW Multiplier

.DEF error = 		R24
.DEF error_old = 	R25

; ========================================================================================
;   S R A M   D E F I N I T I O N S
; ========================================================================================
.DSEG
.ORG  SRAM_START

RX_BUFF:	.byte	10		;SRAM used for communication parameters
SYSTEMSTATE:	.byte	10		;SRAM used for storing the system state (for pc feedback)


; ========================================================================================
;   R E S E T   A N D   I N T   V E C T O R S
; ========================================================================================
;
.CSEG

.ORG 0x0000
	jmp Main ; Reset vector

.ORG 	INT0addr
	reti ; IRQ0 Handler

.ORG 	INT1addr
	reti ; IRQ1 Handler

.ORG 	PCI0addr
	reti ; PCINT0 Handler

.ORG 	PCI1addr
	reti ; PCINT1 Handler

.ORG 	PCI2addr
	reti ; PCINT2 Handler

.ORG 	WDTaddr
	reti ; Watchdog Timer Handler

.ORG 	OC2Aaddr
	reti ; Timer2 Compare A Handler

.ORG 	OC2Baddr
	reti ; Timer2 Compare B Handler

.ORG 	OVF2addr
	reti ; Timer2 Overflow Handler

.ORG 	ICP1addr
	reti ; Timer1 Capture Handler

.ORG 	OC1Aaddr
	jmp	TIMER1A ; Timer1 Compare A Handler

.ORG 	OC1Baddr
	jmp	TIMER1B ; Timer1 Compare B Handler

.ORG 	OVF1addr
	reti ; Timer1 Overflow Handler

.ORG 	OC0Aaddr
	reti ; Timer0 Compare A Handler

.ORG 	OC0Baddr
	reti ; Timer0 Compare B Handler

.ORG 	OVF0addr
	jmp	TIMER0 ; Timer0 Overflow Handler

.ORG 	SPIaddr
	reti ; SPI Transfer Complete Handler

.ORG 	URXCaddr
	jmp USART_RX ; USART, RX Complete Handler

.ORG 	UDREaddr
	reti ; USART, UDR Empty Handler

.ORG 	UTXCaddr
	reti ; USART, TX Complete Handler

.ORG 	ADCCaddr
	reti ; ADC Conversion Complete Handler

.ORG 	ERDYaddr
	reti ; EEPROM Ready Handler

.ORG 	ACIaddr
	reti ; Analog Comparator Handler

.ORG 	TWIaddr
	reti ; 2-wire Serial Interface Handler

.ORG 	SPMRaddr
	reti ; Store Program Memory Ready Handler

;
; ========================================================================================
;   I N T E R R U P T   S E R V I C E S
; ========================================================================================
;
;-------------------------------------------------
; USART RX Interrupt Handler
;-------------------------------------------------
USART_RX:

	SAVE_CONTEXT
	;-----------------------------------------	
								
	ldi	XH, HIGH(RX_BUFF)	;Pointer initialization for storing parameters
	ldi	XL, LOW(RX_BUFF)
	
	lds rxbuff, UDR0		;This actually loads the data

	mov	count, rxbuff		;Load the number of parameters to wait for
	andi	count,0xF0
	swap	count
	
	;Wait for 'count' parameters via serial port
	;Keep reading until 'count' parameters are stored. Then call the respective Handler
read:
	cpi	count,0
	breq	cont
	
	
data:					;Wait for the new one
	lds 	rmp,UCSR0A
	sbrs 	rmp, RXC0
	rjmp 	data

	LOAD	rmp,UDR0		;Store it in the table and Increment the pointer
	st	X+,rmp

	dec	count
	jmp	read

cont:
	;Call the respective handler	
	andi	rxbuff,0x0F		;Mask for getting the Handler name from the Header

	;Indirect call stuff
	ldi	ZH,HIGH(COMMANDS_TABLE<<1)
	ldi	ZL,LOW(COMMANDS_TABLE<<1)

	lsl	rxbuff

	add	ZL,rxbuff
	ldi	rmp,0
	adc	ZH,rmp

	lpm	rmp,Z+
	lpm	rmp2,Z

	mov	ZH,rmp
	mov	ZL,rmp2

	icall				;Indirect Call to what is pointed by ZH:ZL

	;-----------------------------------------
	RESTORE_CONTEXT
	reti

;-------------------------------------------------
; TIMER1A Interrupt Handler
;-------------------------------------------------

TIMER1A:
	SAVE_CONTEXT
; entra a los 20ms (0ms)

; - PORTD como salida
    ldi rmp, 0xFF
    STORE DDRD, rmp

; CANAL A Y B en 1	
	ldi rmp, 0x60
    STORE PORTD, rmp

; -pongo el canalB dentro de 1ms
    LDI rmp, 0x05
    STORE OCR1BH, rmp
    LDI rmp, 0xDC
    STORE OCR1BL, rmp

; - conecta el clock del PWM0
;    LDI rmp, 0x04
;    STORE TCCR0B, rmp
	RESTORE_CONTEXT
    reti

;-------------------------------------------------
; TIMER1B Interrupt Handler
;-------------------------------------------------

TIMER1B:
	SAVE_CONTEXT
; entra a los 2ms
	
	LOAD rmp, OCR1BH
	cpi	rmp,0x05
	breq int_1ms
	jmp int_2ms

int_1ms:
; Si entro en 1ms
;-------------------------------------
; - PORTD como entrada
;    ldi rmp, 0x00
;    STORE DDRD, rmp

; - conecta el clock del PWM0
    LDI rmp, 0x03
    STORE TCCR0B, rmp

; -pongo el canalB en 2ms
    LDI rmp, 0x0B
    STORE OCR1BH, rmp
    LDI rmp, 0xB8
    STORE OCR1BL, rmp
	jmp final
;-------------------------------------	

int_2ms:
; Si entro en 2ms
;-------------------------------------
; - PORTD = 0
    ldi rmp, 0x00
	out PORTD,rmp

    ldi rmp, 0x00
    STORE DDRD, rmp

; - conecta el clock del PWM0
;    LDI rmp, 0x04
;    STORE TCCR0B, rmp

; -pongo el canalB en 1,3ms
;    LDI rmp, 0x4C
;    STORE OCR1BH, rmp
;    LDI rmp, 0x2C
;    STORE OCR1BL, rmp
;-------------------------------------	
	//sbr flags,F_PID_GO
final:
	RESTORE_CONTEXT
    reti

;-------------------------------------------------
; TIMER0 Interrupt Handler
;-------------------------------------------------

TIMER0:
	SAVE_CONTEXT
; - desconecta el clock del PWM0
    LDI rmp, 0x00
    STORE TCCR0B, rmp

	sbr flags,F_PID_GO

	RESTORE_CONTEXT
    reti

;
; ========================================================================================
;   M A I N    P R O G R A M    I N I T
; ========================================================================================
;
MAIN:
	clr	flags			;Clear ALL system flags

	ldi	rmp,0			;Generic Set Point, asuming 0 degress.
	mov	sp,rmp

	ldi	rmp,0			;Set kd,kp,ki. Now they are setted in 0 becuase calibration is needed.
	mov	kp,rmp
	mov	kd,rmp
	mov	ki,rmp

	ldi rmp,0			;Non Real Roll value, asuming 0 degress. Just in case the inertia sensor isn't attached
	mov roll, rmp

; Init stack
	ldi rmp, HIGH(RAMEND) 		;Init MSB stack
	out SPH,rmp
	ldi rmp, LOW(RAMEND) 		;Init LSB stack
	out SPL,rmp

; Init Port B
	ldi rmp,0xFF 			;Direction Port B
	out DDRB,rmp
	
; Init Port D
  	ldi rmp,0xFF 			;Direction Port D
  	out DDRD,rmp

	call PWM_Init			;Init PWM Modules
	call USART_Init			;Init USART Module

	ldi rmp, MIN			;Set all motors at 0%
    STORE OCR0A, rmp
    STORE OCR0B, rmp

	call BLINK			;Startup BLINK

	sei				;enable interrupts


wait:					;Wait for start signal from controller
	sbrs flags,F_START_BIT
	jmp wait

	cbr	flags,F_START		;Clear F_START flag for the next wait structure

;Welcome String
	ldi ZH, high(2*string)		;inicializa el z pointer con la posicion de la tabla
	ldi ZL, low(2*string)	

WELCOME:	
	lpm txbuff, Z+			;load from program memory lo que apunte r16
	cpi txbuff,0			;compara r16 con 0 para ver si termino la cadena
	breq ppal			;si son iguale->branch a ppal, sino sigue
	call USART_Transmit		;transmite lo que haya en r16
	rjmp WELCOME			;busca el siguiente caracter
		
; ========================================================================================
;   P R O G R A M    L O O P
; ========================================================================================
;
;Programa principal despues de inicializaciones
PPAL:
	ldi rmp, MIN			;Set all motors at 0%
    STORE OCR0A, rmp
    STORE OCR0B, rmp

PID:

;Abort Check
	sbrs flags,F_START_BIT
	jmp	PPAL

;PID_GO Check
	sbrs flags,F_PID_GO_BIT
	jmp	PID

; *************************************************
;   				    P I D
; *************************************************

	mov error, sp			;error = setpoint - roll
	sub	error,roll

; -------------------------------------------------
;   PROPORTIONAL
; -------------------------------------------------	
	
	mov rmp, error			;pterm = error * Kp
	fmulsu rmp,kp
	mov pterm,r1

; -------------------------------------------------
;   DERIVATIVE
; -------------------------------------------------

	mov rmp, error			;dterm = (error - error_old) * Kd
	sub rmp, error_old
	fmulsu rmp,kd
	mov dterm,r1

; -------------------------------------------------
;   INTEGRATIVE
; -------------------------------------------------

	mov rmp, istate			;istate = istate + error
	add rmp, error			;iterm = istate * Kd

	cpi rmp, MAX_I
	brlo NO_CLIP_MAX
	ldi rmp2, MAX_I
	mov istate, rmp2
	jmp FIN_CLIP

NO_CLIP_MAX:
	cpi rmp, MIN_I
	brsh NO_CLIP_I_MIN
	ldi rmp2, MIN_I
	mov istate, rmp2
	jmp FIN_CLIP

NO_CLIP_I_MIN:
	mov istate, rmp

FIN_CLIP:
	mov rmp, istate
	fmulsu rmp,ki
	mov	iterm,r1

; -------------------------------------------------
;   SET PWM accordingly
; -------------------------------------------------

	ldi	rmp, PWMQ		;MOTOR_A = PWMQ + pterm + iterm - dterm
	add	rmp,pterm
	add	rmp,iterm
	add	rmp,dterm //estaba en sub
	call SET_PWM_A				


	ldi	rmp, PWMQ		;MOTOR_B = PWMQ - pterm - iterm + dterm
	sub	rmp,pterm
	sub	rmp,iterm
	sub	rmp,dterm  //estaba en add
	call SET_PWM_B

; -------------------------------------------------
;   Send System Status for Feedback
; -------------------------------------------------

	SEND_CHAR 'D'			;Header

	SEND_REGISTER kp		;Kp Value
	SEND_REGISTER ki		;Ki	Value
	SEND_REGISTER kd		;Kd Value
	SEND_REGISTER sp		;Set Point Value
	SEND_REGISTER error		;Set Point Value
	SEND_REGISTER roll		;Set Point Value
	SEND_REGISTER pterm		;pterm Value
	SEND_REGISTER iterm		;iterm Value
	SEND_REGISTER dterm		;dterm Value
	
	SEND_REGISTER pwma		;PWM_A Value
	SEND_REGISTER pwmb		;PWM_B Value

	call DELAY_1ms
	call DELAY_1ms
	call DELAY_1ms

	//SEND_CHAR STOP_DEBUG		;End of transmition
	
; -------------------------------------------------
;   Loop stuff
; -------------------------------------------------

	mov	error_old, error	;Save the error as the old one
	 
	jmp PID
	 
;
; ========================================================================================
;	S U B R U T I N E S
; ========================================================================================
;

; -------------------------------------------------
;   PWM Initialization routine
; -------------------------------------------------

PWM_Init:

; configuracion timer0
; modo fast PWM
; con T = 5.461ms
; en el ov. se deshabilita el PWM
; y se vuelve a habilitar cada 20ms

    LDI rmp, 0xA3
    STORE TCCR0A, rmp
    LDI rmp, 0x03
    STORE TCCR0B, rmp
    LDI rmp, 0x01
    STORE TIMSK0, rmp

; configuracion timer1
; cuenta ascendente
; modo comparador
; cuenta de 0x0000 a 0x7530 que son 20ms
; y compara a los 2ms y a los 20ms
; clkio = 12mhz y va con prescaler /8 (8/12us por tic)
; en la int. de este timer de habilita el PWM (20ms)
; y lo deshabilita (2ms)

    LDI rmp, 0x75
    STORE OCR1AH, rmp
    LDI rmp, 0x30
    STORE OCR1AL, rmp   

    LDI rmp, 0x05
    STORE OCR1BH, rmp
    LDI rmp, 0xDC
    STORE OCR1BL, rmp

    LDI rmp, 0x0A
    STORE TCCR1B, rmp

    LDI rmp, 0x06
    STORE TIMSK1, rmp

	ret

; -------------------------------------------------
;   USART Initialization routine
; -------------------------------------------------

USART_Init:
	
; Set baud rate	

	;Configure USAR Baudrate

	ldi	rmp,HIGH(fosc/((16*baudrate))-1)	
	ldi	rmp2,LOW(fosc/((16*baudrate))-1)
	
	sts UBRR0H, rmp
	sts UBRR0L, rmp2

	;Enable receiver and transmitter

	ldi rmp, (1<<RXEN0)|(1<<TXEN0)|(1<<RXCIE0) 
	STORE UCSR0B,rmp

	
	;Set frame format: 8data, 1stop bit , no parity, async
	ldi rmp, (3<<UCSZ00)					
	STORE UCSR0C,rmp

	ret

; -------------------------------------------------
;   USART Transmition routine
; -------------------------------------------------

USART_Transmit:
	
	SKBS UCSR0A,UDRE0,rmp		;Wait for empty transmit buffer
	rjmp USART_Transmit
	
	STORE UDR0,txbuff		;Put txbuff into buffer, sends the data

	ret


; -------------------------------------------------
;   SET PWM A
; -------------------------------------------------

SET_PWM_A:
;	cpi	rmp,MIN
;	brlo CLIP_A_L

	cpi	rmp,MAX
	brsh CLIP_A_H

	mov	pwma,rmp
	STORE OCR0A,rmp	
	ret

CLIP_A_L:
	ldi	rmp, MIN
	mov	pwma,rmp
	STORE OCR0A,rmp
	ret

CLIP_A_H:
	ldi	rmp, MAX
	mov	pwma,rmp
	STORE OCR0A,rmp
	ret

; -------------------------------------------------
;   SET PWM B
; -------------------------------------------------

SET_PWM_B:
;	cpi	rmp,MIN
;	brlo CLIP_B_L

	cpi	rmp,MAX
	brsh CLIP_B_H

	mov	pwmb,rmp
	STORE OCR0B,rmp

	ret

CLIP_B_L:
	ldi	rmp, MIN
	mov	pwmb,rmp
	STORE OCR0B,rmp
	ret

CLIP_B_H:
	ldi	rmp, MAX
	mov	pwmb,rmp
	STORE OCR0B,rmp
	ret


; -------------------------------------------------
;   Buzzer Blink
; -------------------------------------------------

BLINK:
	sbi PORTB,BUZZER			
	call DELAY_1ms
	cbi PORTB,BUZZER
	call DELAY_1ms
	sbi PORTB,BUZZER
	call DELAY_1ms
	cbi PORTB,BUZZER
	call DELAY_1ms
	ret

; -------------------------------------------------
;   Generic Delay 
; -------------------------------------------------

; (WARNING: It's not a 1 second delay anymore)

DELAY_1ms:                                            
           ldi rmp, $AA    	;$0A = 10 (decimal)
L2:        ldi rmp2, $F2    	;$F2 = 242 (decimal)
L3:        dec rmp2
           brne L3        
           dec rmp
           brne L2                                   
           nop            
           nop            
           nop            
           nop            
           nop                                      
           ret

;
; ========================================================================================
;	C O M M A N D S   H A N D L E R S
; ========================================================================================
;
; -------------------------------------------------
;   Command NOP Handler
; -------------------------------------------------
COMMAND_NOP_HND:
	call BLINK
	ret

; -------------------------------------------------
;   Command Angles Handler
; -------------------------------------------------
COMMAND_ANGLES_HND:
		
	ldi	XH,HIGH(RX_BUFF)
	ldi	XL,LOW(RX_BUFF)

	ld	roll,X+
	
	ret

; -------------------------------------------------
;   Command SETPWM Handler
; -------------------------------------------------
COMMAND_SET_PWM_HND:

	ldi	XH,HIGH(RX_BUFF)
	ldi	XL,LOW(RX_BUFF)

	ld	rmp,X+
	call SET_PWM_A
	
	ld	rmp,X+
	call SET_PWM_B

	ret


; -------------------------------------------------
;   Command SET_POINTS Handler
; -------------------------------------------------
COMMAND_SET_POINTS_HND:

	ldi	XH,HIGH(RX_BUFF)
	ldi	XL,LOW(RX_BUFF)

	ld	sp,X+

	ret

; -------------------------------------------------
;   Command JOYSTICK Handler
; -------------------------------------------------

COMMAND_JOYSTICK_HND:
		
	ldi	XH,HIGH(RX_BUFF)
	ldi	XL,LOW(RX_BUFF)
	ld	rmp,X

	andi rmp,1
	andi flags,254
	or flags,rmp

	ret

; -------------------------------------------------
;   Command KPARAMS Handler
; -------------------------------------------------

COMMAND_KPARAMS_HND:

	ldi	XH,HIGH(RX_BUFF)
	ldi	XL,LOW(RX_BUFF)
	ld	kp,X+
	ld	kd,X+
	ld	ki,X

	ret	
;
; ========================================================================================
;   T A B L E S
; ========================================================================================
;

string: 
	.DB START_DEBUG,"QuadRotor Inicializado",10,STOP_DEBUG,0

commands_table: 
	.DB HIGH(COMMAND_NOP_HND),LOW(COMMAND_NOP_HND),HIGH(COMMAND_ANGLES_HND),LOW(COMMAND_ANGLES_HND),HIGH(COMMAND_SET_POINTS_HND),LOW(COMMAND_SET_POINTS_HND),HIGH(COMMAND_JOYSTICK_HND),LOW(COMMAND_JOYSTICK_HND),HIGH(COMMAND_SET_PWM_HND),LOW(COMMAND_SET_PWM_HND),HIGH(COMMAND_KPARAMS_HND),LOW(COMMAND_KPARAMS_HND)
